package com.sensors;

import android.util.Log;

import com.DataProcess.Robot;

/**
 * Created by tjpu on 2016/10/10.
 */
public class Encoder {
    static String thisLogTag = "Encoder";

    public static double PERIOD = 0.1;    //编码器周期
    public static int CPR = 3200;    //轮子每转一圈的计数
    public static double CONVERT_TO_METRIC = Robot.WHEEL_PERIMETER / CPR / PERIOD;

    public  double receivePeriod = 0.1;
    long lastTime = System.currentTimeMillis();
    public short[] origin = {0, 0};
    double[] metricUnit = {0.0, 0.0};

    public int notReceive = 0;
    public boolean isNotReceive = false;

    public void putOrigin(short[] encoder) {
        this.origin[0] = encoder[0];
        this.origin[1] = encoder[1];

        convert();
        judge();
        filter();

        notReceive = 0;
        isNotReceive = false;
        receivePeriod = (System.currentTimeMillis() - lastTime) / 1000.0;
        lastTime = System.currentTimeMillis();
        Log.i(thisLogTag,"receive period: "+receivePeriod);
    }

    public double[] getMetric() {
        double[] returnData = new double[metricUnit.length];
        System.arraycopy(metricUnit,0,returnData,0,returnData.length);
        return returnData;
    }

    void convert() {
        metricUnit[0] = origin[0] * CONVERT_TO_METRIC;
        metricUnit[1] = origin[1] * CONVERT_TO_METRIC;
    }

    void judge() {
    }

    void filter() {
    }

    public void checkReceive() {
        notReceive++;
        if (notReceive > 3 && !isNotReceive) {
            isNotReceive = true;
            Log.e(thisLogTag,"can't receive data");
        }
    }
}
